//
// Created by lee on 24-6-26.
//
#include "decode.h"

static size_t  _rx_leng{0};
static size_t _rx_count{0};
static uint16_t _rx_ck{0};
static uint8_t _len_h{0};
static agx_decode_state_t _agx_decode_state{AGX_DECODE_HEAD1};

uint16_t get_checksum(const uint8_t *buff, uint16_t sz) {
    uint16_t check_sum = 0;
    if (buff == nullptr) return 0;
    for (int i = 0; i < sz ; i++ )
    {
        check_sum += buff[i];
    }
    return check_sum;
}

void addByteToChecksum(uint8_t b)
{
    _rx_ck += b;
}

void decodeInit()
{
    _rx_ck = 0;
    _rx_count = 0;
    _agx_decode_state = AGX_DECODE_HEAD1;
}

int parseChar(uint8_t b, uint8_t *packet, RECV_TYPE &recv_type, uint16_t& length) {
    int ret = 0;
    switch (_agx_decode_state) {
        case AGX_DECODE_HEAD1:
            if (b == 0x55) {
                _agx_decode_state = AGX_DECODE_HEAD2;
            } else {
                decodeInit();
            }
            break;

        case AGX_DECODE_HEAD2:
            if (b == 0xaa) {
                _agx_decode_state = AGX_PAYLOAD_LEN_H;
            } else {
                decodeInit();
            }
            break;

        case AGX_PAYLOAD_LEN_H:
            _len_h = b;
            addByteToChecksum(b);
            _agx_decode_state = AGX_PAYLOAD_LEN_L;
            break;

        case AGX_PAYLOAD_LEN_L: {
            addByteToChecksum(b);
            uint16_t len = (_len_h << 8) | b;
            if (len > 0) {
                _rx_leng = len;
                _agx_decode_state = AGX_DECODE_MSGID;
            } else {
                decodeInit();
            }
            break;
        }

        case AGX_DECODE_MSGID:
            if (b == 0x7c) {
                recv_type = agx_send_visual_inertial_nav_type;
                addByteToChecksum(b);
                _agx_decode_state = AGX_DECODE_PAYLOAD;
            } else if (b==0x7d){
                recv_type = px4_transmit2pod_pod_control_type;
                addByteToChecksum(b);
                _agx_decode_state = AGX_DECODE_PAYLOAD;
            }else if (b==0x2e){
                recv_type = agx_2_px4_route_type;
                length = _rx_leng;
                addByteToChecksum(b);
                _agx_decode_state = AGX_DECODE_PAYLOAD;
            }

            else {
                decodeInit();
                printf("got wrong msgid, get %X\n", b);
            }
            break;

        case AGX_DECODE_PAYLOAD:
            packet[_rx_count] = b;
            _rx_count++;
            addByteToChecksum(b);

            if (_rx_count >= _rx_leng) {
                _agx_decode_state = AGX_DECODE_CHECKSUM_L;
            }
            break;

        case AGX_DECODE_CHECKSUM_L:
            if (b == (_rx_ck & 0x00ff)) {
                ret = 1;
            } else {
                ret = -1;
            }
            decodeInit();
            break;

        default:
            decodeInit();
            break;
    }

    return ret;  // ret = 1 正确，-1 错误，0 未进行到高位校验和
}